#pragma once

#include "Common.h"

#define PIN_ECHO  9 // GELB
#define PIN_TRIGGER 8 // BLAU

class UltrasonicReceiver
{

public:

  inline static void Init() 
  {
    pinMode(PIN_TRIGGER, OUTPUT);
    pinMode(PIN_ECHO, INPUT);

    NextCheckTimeInMs = 0;
  }

  inline static void UpdateNextCheckTime() 
  {
    NextCheckTimeInMs = globals::CurrentLoopTimeInMs + 50;
  }

  inline static bool IsCheckTimeReached() 
  {
    return (NextCheckTimeInMs < globals::CurrentLoopTimeInMs);
  }

  inline static void DoUpdateState() 
  {
    long new_distance;

    // --- do measurement ---

    /* The following trigPin/echoPin cycle is used to determine the distance of the nearest object by bouncing soundwaves off of it. */
    digitalWrite(PIN_TRIGGER, LOW); 
    delayMicroseconds(2); 

    digitalWrite(PIN_TRIGGER, HIGH);
    delayMicroseconds(10);     
    digitalWrite(PIN_TRIGGER, LOW);
    Duration = pulseIn(PIN_ECHO, HIGH);

    //Calculate the distance (in cm) based on the speed of sound.
    new_distance = (long)((Duration / 58.2) * 10.0);

    if (new_distance < MinimumRange)  {
      // new_distance = MinimumRange;
      return;
    }
    
    if (new_distance > MaximumRange) {
      // new_distance = MaximumRange;
      return;
    }

    if ( abs(Distance - new_distance) > 5 ) {
      IsStateChanged = true;
      Distance = new_distance;
    }

  }

  inline static void UpdateState() 
  {
    if ( IsCheckTimeReached() ) {
      DoUpdateState();
      UpdateNextCheckTime(); 
    }
  }

  static unsigned long NextCheckTimeInMs;  
  static bool IsStateChanged;  
  static int MaximumRange; // Maximum range needed
  static int MinimumRange; // Minimum range needed
  static long Duration; 
  static long Distance; // Duration used to calculate distance in millimeters

};






